﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;
using JigLibX.Collision;
using JigLibX.Physics;
using JigLibX.Geometry;
using JigLibX.Math;

namespace DARE
{
    public class CCharacterController : APhysicController
    {

    #region fields

        protected Vector3 m_desiredVelocity;
        protected float m_moveSpeed;

        protected Vector3 m_moveTo;
        protected bool m_processMove;
        protected float m_distance;

        protected float m_yawDirection;
        protected float m_moveSensibility;

        protected Vector3 m_oldPosition;
        protected bool m_stopWhenStuck;

        protected bool m_stopped;
        protected bool m_needUpdateDistanceFromGround;
        protected float m_distanceFromGround;

        protected float m_borderHeight;

    #endregion

    #region properties

        public Vector3 DesiredVelocity 
        { 
            get { return m_desiredVelocity; }
            set { m_desiredVelocity = value; }
        }

        public float MoveSpeed
        {
            get { return m_moveSpeed; }
            set { m_moveSpeed = value; }
        }

        public bool IsMoving
        {
            get { return m_processMove; }
        }
        
        public float YawDirection
        {
            get { return m_yawDirection; }
        }

        public float MoveSensibility
        {
            get { return m_moveSensibility; }
            set { m_moveSensibility = value; }
        }

        public bool StopWhenStuck
        {
            get { return m_stopWhenStuck; }
            set { m_stopWhenStuck = value; }
        }

        public float DistanceFromGround
        {
            get
            {
                if (m_needUpdateDistanceFromGround)
                {
                    if (m_node.Render3D == null)
                        m_distanceFromGround = -1000;
                    else
                    {
                        Vector3 velocity = m_body.Velocity;
                        Vector3 pos = m_body.Position;
                        BoundingBox bb = m_node.Entity.Physic.Skin.WorldBoundingBox;
                        float height = bb.Max.Y - bb.Min.Y;

                        m_body.MoveTo(pos + Vector3.UnitY * (height / 2) + Vector3.UnitY * 0.1f, m_body.Orientation);
                        CRayCast.CIntersectInfo iInfo = m_node.Render3D.RayCast.IntersectFirst(pos, -Vector3.UnitY * 10.0f, 100);
                        m_body.MoveTo(pos, m_body.Orientation);
                        m_body.Velocity = velocity;

                        m_distanceFromGround = iInfo == null ? -1000 : pos.Y - iInfo.pos.Y - (height / 2);
                    }
                    m_needUpdateDistanceFromGround = false;
                }
                return m_distanceFromGround;
            }
        }

        public bool IsOnTheGround
        {
            get { return DistanceFromGround != -1000 && DistanceFromGround < 0.2f; }
        }

        /// <summary>
        /// Do not block if set to 0.
        /// </summary>
        public float BlockAtBorderHeight
        {
            get { return m_borderHeight; }
            set { m_borderHeight = value; }
        }

    #endregion

    #region ctor

        /// <summary>
        /// Dont use it :D
        /// </summary>
        public CCharacterController()
        {
            Initialize();
        }

        public CCharacterController(CNode node) : 
            base(node)
        {
            Initialize();
        }

        public CCharacterController(CEntity entity) : 
            base(entity.Node)
        {
            Initialize();
        }

        public virtual void Initialize()
        {
            // initialize members
            m_desiredVelocity = Vector3.Zero;
            m_moveSpeed = 4.0f;
            m_moveTo = Vector3.Zero;
            m_processMove = false;
            m_distance = 0.0f;
            m_moveSensibility = 0.1f;
            m_oldPosition = Vector3.Zero;
            m_stopWhenStuck = true;
            m_needUpdateDistanceFromGround = true;
            m_borderHeight = 0.4f;

            // body properties
            if (m_body != null)
            {
                MaterialProperties mp = m_body.CollisionSkin.GetMaterialProperties(0);
                mp.DynamicRoughness = 0.0f;
                mp.StaticRoughness = 0.0f;
                mp.Elasticity = 0.0f;
                m_body.CollisionSkin.SetMaterialProperties(0, mp);

                m_body.SetBodyInvInertia(0, 0, 0);
                m_body.Mass = 5.0f;
                m_body.EnableBody();
                m_body.AllowFreezing = false;
            }
        }

    #endregion

    #region methods

        public void Stop()
        {
            m_desiredVelocity.X = 0.0f;
            m_desiredVelocity.Z = 0.0f;
            m_processMove = false;
            m_stopped = true;
            
        }

        public void WorldMove(Vector3 move)
        {
            m_desiredVelocity = move * m_moveSpeed;
        }

        public void Jump(int force)
        {
            if (IsOnTheGround)
                m_body.ApplyWorldImpulse(Vector3.UnitY * force);
        }

        public virtual void MoveTo(Vector3 position)
        {            
            Vector3 moveToXZ = new Vector3(position.X, 0, position.Z);
            Vector3 positionXZ = new Vector3(m_body.Position.X, 0, m_body.Position.Z);
            Vector3 travel = moveToXZ - positionXZ;
            m_distance = travel.Length();

            if ((m_distance <= m_moveSensibility) || (m_distance <= 0))
                return;

            m_processMove = true;
            m_moveTo = position;

            float adj = travel.X;
            float opp = travel.Z;
            float hyp = m_distance;

            float x, y;
            float angle = 0;

            x = (float)Math.Acos((double)(adj / hyp));
            y = (float)Math.Asin((double)(opp / hyp));

            angle = y <= 0 ? x : -x;
            angle = angle;

            //m_yawDirection = MathHelper.WrapAngle(angle);

            m_yawDirection = angle;

            // reset the old position otherwise the character will be detected as stopped
            m_oldPosition = Vector3.Zero;
        }

        public override void UpdateController(float dt)
        {
            // move to
            if (m_processMove == true)
            {
                Vector3 moveToXZ = new Vector3(m_moveTo.X, 0, m_moveTo.Z);
                Vector3 positionXZ = new Vector3(m_body.Position.X, 0, m_body.Position.Z);
                Vector3 travel = moveToXZ - positionXZ;
                float distanceLeft = travel.Length();

                travel.Normalize();
                if (distanceLeft > m_distance)
                {
                    m_processMove = false;
                    m_stopped = true;
                    //m_body.MoveTo(new Vector3(moveToXZ.X, m_body.Position.Y, moveToXZ.Z), m_body.Orientation);
                    m_desiredVelocity = new Vector3(0, m_body.Velocity.Y, 0);
                }
                else
                    m_desiredVelocity = travel * m_moveSpeed;
                m_distance = distanceLeft;
            }

            // normalize the max speed
            if (Math.Abs(m_desiredVelocity.X) > m_moveSpeed)
                m_desiredVelocity.X = m_moveSpeed * Math.Sign(m_desiredVelocity.X);
            if (Math.Abs(m_desiredVelocity.Z) > m_moveSpeed)
                m_desiredVelocity.Z = m_moveSpeed * Math.Sign(m_desiredVelocity.Z);

            // avoid the small jump when movement stop
            float velY = m_body.Velocity.Y;
            if ((m_desiredVelocity.X == 0) && (m_desiredVelocity.Z == 0) && m_stopped)
                velY = Math.Sign(velY) > 0 ? 0 : velY;
            m_stopped = false;

            // stop the character if it's stuck
            if (m_stopWhenStuck && !CDare.Instance.Config.Time.IsPaused && 
                m_processMove && (m_oldPosition == m_body.Position))
                Stop();

            // ================================ RAY AROUND THE CHAR ================================

            if (m_borderHeight != 0)
            {
                int nbRay = 16;
                float distRay = 0.4f;
                int nbDir = 0;
                int direction;

                //float depthDetection = 0.4f;

                float angle = 0;
                float dist0 = 0;
                float dist1 = 0;

                float velocityXZLength = m_desiredVelocity.Length();
                float limitLeft = 0;
                float limitRight = 0;
                float yaw = m_yawDirection;
                float step = MathHelper.TwoPi / (float)nbRay;

                if (yaw < 0)
                    yaw = MathHelper.Pi + (MathHelper.Pi + yaw);
                yaw += MathHelper.TwoPi;

                //CDare.Instance.DebugMgr.DebugConsole.WriteLine("yaw = " + yaw.ToString());

                Vector3 adjustVelocity = Vector3.Zero;

                for (int i = 0; i <= nbRay; ++i)
                {
                    if (i < nbRay)
                    {
                        angle = (float)i * step;
                        dist0 = DistFromChar(distRay / 2, MathHelper.WrapAngle(angle));
                        dist1 = DistFromChar(distRay, MathHelper.WrapAngle(angle));
                        limitLeft = angle + (step + MathHelper.TwoPi);
                        limitRight = angle - (step + MathHelper.TwoPi);
                    }
                    if ((yaw > limitRight && yaw < limitLeft) ||
                        ((i == nbRay - 1) && (nbDir == 1)))
                    {
                        direction = i == 8 ? 0 : i;
                        if (nbDir < 2)
                        {
                            nbDir += 1;

                            //CDare.Instance.DebugMgr.DebugConsole.WriteLine("  We are moving in the direction of the ray " + direction.ToString() + " !!");
                            //m_yawDirection = ...
                        }
                    }
                    if (i >= nbRay)
                        break;
                    if ((dist1 < -m_borderHeight) && (dist0 < -m_borderHeight))
                    {
                        //CDare.Instance.DebugMgr.DebugConsole.WriteLine("  " + i.ToString() + ", dist0 < -0.4");

                        // si on se retrouve a pousser le personnage dans le cas d'une pente ou son cul est dans le vide, il sera propulse
                        // prendre en compte l'angle ET m_yawDirection du perso !
                        // test si le trou est devant lui

                        Vector3 opposite = Vector3.Transform(Vector3.UnitX,
                            Quaternion.CreateFromYawPitchRoll(MathHelper.WrapAngle(angle), 0, 0));

                        adjustVelocity.X = MathHelper.Clamp(adjustVelocity.X - opposite.X * m_moveSpeed, -m_moveSpeed, m_moveSpeed);
                        adjustVelocity.Z = MathHelper.Clamp(adjustVelocity.Z - opposite.Z * m_moveSpeed, -m_moveSpeed, m_moveSpeed);
                    }
                }

                if (m_processMove && IsOnTheGround)
                    m_desiredVelocity += adjustVelocity;
            }

            // =====================================================================================

            m_body.Velocity = new Vector3(m_desiredVelocity.X, velY, m_desiredVelocity.Z);

            base.UpdateController(dt);

            if (!CDare.Instance.Config.Time.IsPaused)
                m_oldPosition = m_body.Position;

            //CDare.Instance.DebugMgr.DebugConsole.WriteLine("velocity = " + m_body.Velocity.ToString());

            m_needUpdateDistanceFromGround = true;
        }

    #endregion

    #region utils

        public float DistFromChar(float dist, float angle)
        {
            if (m_node.Render3D == null)
                return -1000;

            float rayHeight = 10;
            Vector3 rayPos = m_body.Position + Vector3.Transform(Vector3.UnitX * dist, 
                Quaternion.CreateFromYawPitchRoll(angle, 0, 0));
            Vector3 velocity = m_body.Velocity;
            Vector3 pos = m_body.Position;
            BoundingBox bb = m_node.Entity.Physic.Skin.WorldBoundingBox;
            float height = bb.Max.Y - bb.Min.Y;

            rayPos.Y = rayHeight;
            m_body.MoveTo(pos + Vector3.UnitY * (height / 2) + Vector3.UnitY * rayHeight + 
                          Vector3.UnitY * 0.1f, m_body.Orientation);
            CRayCast.CIntersectInfo iInfo = m_node.Render3D.RayCast.IntersectFirst(rayPos, -Vector3.UnitY * rayHeight * 2, 100);
            m_body.MoveTo(pos, m_body.Orientation);
            m_body.Velocity = velocity;

            return iInfo == null ? -1000 : -((pos.Y - (height / 2)) - iInfo.pos.Y);
        }

    #endregion

    }
}
